
from pygame.joystick import Joystick
import pygame
import serial
import math
import string
import time
#from numpy.ma.core import floor

#pygame init
pygame.init()
pygame.joystick.init()
try:
	j = Joystick(0) # create a joystick instance
	j.init() # init instance
	print 'Enabled joystick: ' + j.get_name()
except pygame.error:
	print 'no joystick found.'

#serial init
s = serial.Serial(port='COM8', baudrate=57600, timeout=1)

def writeShort( val ):
	s.write( chr( val & 0xFF ) )
	s.write( chr( (val >> 8) & 0xFF ) )
	
def writeAxis( val ):
	writeShort( math.floor(( val + 1.0 ) / 2 * (1<<15)) )
	
def writeVal( val ):
	s.write( chr(int(math.floor(((val+1.0)/2.0) * 255))) )
	

try:
	row = 0
	yaw = 0
	pit = 0
	z = 0
	eStop = 0
	while 1:
		e = pygame.event.poll() 
		if e.type == pygame.JOYAXISMOTION: # 7
			z, yaw , pit, row = j.get_axis(1) , j.get_axis(2), j.get_axis(3), j.get_axis(4)
			eStop = j.get_button(0) | j.get_button(1) | j.get_button(2) | j.get_button(3)
			z = (abs(z)*2)-1
			if eStop == 0:
				s.write( 'a' )
				writeVal( z )
				writeVal( yaw )
				writeVal( pit )
				writeVal( row )
			if eStop == 1:
				s.write( 'e')
			
		if s.readable() == True:
			print s.readline()
			
			s.flushInput()
			#s.flushOutput()

except KeyboardInterrupt:
	s.close()

